#include "sin.h"
#include <math.h>

#define PI 3.14159265358979f

void SinTrajectory(float t, GaitParams params, float gaitOffset, float &x, float &y)
{
  static float p = 0;
  static float prev_t = 0;

  float stanceHeight = params.stance_height;
  float downAMP = params.down_amp;
  float upAMP = params.up_amp;
  float flightPercent = params.flight_percent;
  float stepLength = params.step_length;
  float FREQ = params.freq;

  p += FREQ * (t - prev_t);
  prev_t = t;

  float gp = fmod((p + gaitOffset), 1.0f);

  if (gp <= flightPercent)
  {
    x = (gp / flightPercent) * stepLength - stepLength / 2.0f;
    y = -upAMP * sin(PI * gp / flightPercent) + stanceHeight;
  }
  else
  {
    float percentBack = (gp - flightPercent) / (1.0f - flightPercent);
    x = -percentBack * stepLength + stepLength / 2.0f;
    y = downAMP * sin(PI * percentBack) + stanceHeight;
  }
}